#include "SilzAstar.h"
#include "game_map.h"

using std::max;
using std::min;
using std::make_heap;
using std::push_heap;
using std::pop_heap;
using std::vector;

USING_NS_CC;

ASGrid::ASGrid(int cols, int rows)
	:_nNumCols(cols)
	,_nNumRows(rows)
	,_pStartNode(nullptr)
	,_pEndNode(nullptr)
	,_vNodes()
{
	int size = _nNumCols * _nNumRows;
	for ( int row = 0; row < _nNumRows; ++row )
	{
		for ( int col = 0; col < _nNumCols; ++col )
		{
			_vNodes.push_back(new ASNode(col, row));
		}
	}
}

ASGrid::~ASGrid()
{
	for ( auto pNode : _vNodes )
	{
        for ( auto pNodeLink : pNode->links )
        {
            delete pNodeLink;
            pNodeLink = nullptr;
        }
        pNode->links.clear();

		delete pNode;
		pNode = nullptr;
	}
	_vNodes.clear();
}

void ASGrid::calculateLinks()
{
	for ( auto pNode : _vNodes )
	{
		initNodeLink(pNode);
	}
}

void ASGrid::setEndNode(int col, int row)
{
	int index = row * _nNumCols + col;
	_pEndNode = _vNodes[index];
}

void ASGrid::setStartNode(int col, int row)
{
	int index = row * _nNumCols + col;
	_pStartNode = _vNodes[index];
}

void ASGrid::setWalkable(int col, int row, bool value)
{
	int index = row * _nNumCols + col;
	_vNodes[index]->walkable = value;
}

ASNode* ASGrid::getNode(int col, int row)
{
	int index = row * _nNumCols + col;
	return _vNodes[index];
}

void ASGrid::initNodeLink(ASNode *node)
{
	int startX = max(0, node->x - 1);
	int startY = max(0, node->y - 1);
	int endX = min(_nNumCols - 1, node->x + 1);
	int endY = min(_nNumRows - 1, node->y + 1);
    for ( int i = startX; i <= endX; i++ )
    {
        for ( int j = startY; j <= endY; j++ )
        {
            ASNode *testNode = getNode(i, j);
            if ( testNode == node || !(testNode->walkable) )
                continue;
            double cost = STRAIGHT_COST;
            if ( node->x != testNode->x && node->y != testNode->y )
                cost = DIAG_COST;
            node->links.push_back(new ASNodeLink(testNode, cost));
        }
    }
}

AStar::AStar(ASGrid *grid)
    :_grid(grid)
    ,_endNode(nullptr)
    ,_startNode(nullptr)
    ,_nowVersion(1)
{
}

bool AStar::findPath()
{
    _endNode = _grid->getEndNode();
    _startNode = _grid->getStartNode();
    _startNode->g = 0.0;
    _open.clear();
    _nowVersion++;
    return search();
}

void AStar::floyd()
{
    if ( _path.size() == 0 )
        return;

    _floydPath = _path;
    int size = _floydPath.size();
    if ( size > 2 )
    {
        ASNode node(0, 0);
        ASNode tempNode(0, 0);
        floydNode(node, *_floydPath[size-1], *_floydPath[size-2]);
        for ( int i = size-3; i >= 0; --i )
        {
            floydNode(tempNode, *_floydPath[i+1], *_floydPath[i]);
            if ( node.x == tempNode.x && node.y == tempNode.y )
            {
                vector<ASNode*>::iterator iter = _floydPath.begin() + (i+1);
                _floydPath.erase(iter);
            }
            else
            {
                node.x = tempNode.x;
                node.y = tempNode.y;
            }
        }
    }
    size = _floydPath.size();
    for ( int i = size-1; i >= 0; --i )
    {
        for ( int j = 0; j <= i-2; ++j )
        {
            if ( floydCrossAble(*_floydPath[i], *_floydPath[j]) )
            {
                for ( int k = i-1; k > j; --k )
                {
                    vector<ASNode*>::iterator iter = _floydPath.begin() + k;
                    _floydPath.erase(iter);
                }
                i = j;
            }
        }
    }
}

bool AStar::search()
{
    if ( !_startNode || !_endNode )
        return false;

    ASNode *pNode = _startNode;
    pNode->version = _nowVersion;
	while ( pNode != _endNode )
    {
        std::vector<ASNodeLink*>::size_type size = pNode->links.size();
		for ( std::vector<ASNodeLink*>::size_type i = 0; i < size; ++i )
        {
            ASNode *pTestNode = pNode->links[i]->node;
            double cost = pNode->links[i]->cost;
			double g = pNode->g + cost;
            double h = euclidian(*pTestNode);
			double f = g + h;
            if ( pTestNode->version == _nowVersion )
            {
				if ( pTestNode->f > f )
                {
					pTestNode->f = f;
					pTestNode->g = g;
					pTestNode->h = h;
					pTestNode->parent = pNode;
				}
			}
            else
            {
				pTestNode->f = f;
				pTestNode->g = g;
				pTestNode->h = h;
				pTestNode->parent = pNode;
				pTestNode->version = _nowVersion;
                _open.push_back(pTestNode);
				push_heap(_open.begin(), _open.end(), AStar::justMin);
			}
					
		}
        if ( _open.size() == 0 )
		{
			return false;
		}
        pop_heap(_open.begin(), _open.end(), AStar::justMin);
        pNode = _open.back();
        _open.pop_back();
	}
	buildPath();
	return true;
}
    
    
bool AStar::floydCrossAble(const ASNode &n1, const ASNode &n2)
{
    vector<Point> vPoints;
    bresenhamNodes(vPoints, Point(n1.x, n1.y), Point(n2.x, n2.y));
    for ( vector<Point>::size_type i = vPoints.size()-2; i > 0; --i )
    {
        ASNode *node = _grid->getNode(vPoints[i].x, vPoints[i].y);
        if ( !node || !node->walkable )
            return false;
    }
    return true;
}

void AStar::bresenhamNodes(std::vector<cocos2d::Point> &result, cocos2d::Point &p1, cocos2d::Point &p2)
{
    result.clear();
    bool isSteep = abs(p2.y - p1.y) > abs(p2.x - p1.x);
	if ( isSteep )
    {
		int temp = p1.x;
		p1.x = p1.y;
		p1.y = temp;
		temp = p2.x;
		p2.x = p2.y;
		p2.y = temp;
	}
	int stepX = p2.x > p1.x ? 1 : (p2.x < p1.x ? -1 : 0);
	int stepY = p2.y > p1.y ? 1 : (p2.y < p1.y ? -1 : 0);
	double deltay = (p2.y - p1.y) / abs(p2.x - p1.x);
	double nowX = p1.x + stepX;
	double nowY = p1.y + deltay;
	if ( isSteep )
    {
        result.push_back(Point(p1.y, p1.x));
	}
    else
    {
		result.push_back(Point(p1.x, p1.y));
	}
	while ( nowX != p2.x )
    {
		int fy = floor(nowY);
		int cy = ceil(nowY);
		if ( isSteep )
        {
			result.push_back(Point(fy, nowX));
		}
        else
        {
			result.push_back(Point(nowX, fy));
		}
		if ( fy != cy )
        {
			if ( isSteep )
            {
				result.push_back(Point(cy, nowX));
			}
            else
            {
				result.push_back(Point(nowX, cy));
			}
		}
		nowX += stepX;
		nowY += deltay;
	}
	if ( isSteep )
    {
		result.push_back(Point(p2.y, p2.x));
	}
    else
    {
		result.push_back(Point(p2.x, p2.y));
	}
}

void AStar::floydNode(ASNode &result, const ASNode &n1, const ASNode &n2)
{
    result.x = n1.x - n2.x;
	result.y = n1.y - n2.y;
}
void AStar::buildPath()
{
    _path.clear();
    ASNode *pNode = _endNode;
    _path.push_back(pNode);
	while ( pNode != _startNode )
    {
		pNode = pNode->parent;
        _path.insert(_path.begin(), pNode);
	}
}

double AStar::euclidian(const ASNode &node)
{
    double dx = node.x - _endNode->x;
	double dy = node.y - _endNode->y;
	return dx * dx + dy * dy;
}

bool AStar::justMin(ASNode *n1, ASNode *n2)
{
    return n1->f > n2->f;
}

SilzAstar::SilzAstar()
    :_grid(nullptr)
    ,_astar(nullptr)
{
}

SilzAstar::~SilzAstar()
{
    delete _astar;
    delete _grid;
}

void SilzAstar::find(vector<Point> &result, float srcX, float srcY, float descX, float descY)
{
    result.clear();
    int descCol = descX;
    int descRow = descY;

    descCol = min(descCol, _grid->getNumCols() - 1);
    descRow = min(descRow, _grid->getNumRows() - 1);

	if ( !_grid->getNode(descCol, descRow)->walkable )
		return;

	_grid->setEndNode(descCol, descRow);
				
	int srcCol = srcX;
    int srcRow = srcY;
	_grid->setStartNode(srcCol, srcRow);
			
    if ( _astar->findPath() )
    {					
		_astar->floyd();
        const vector<ASNode*> &vFloydPath = _astar->getFloyPath();
        for ( auto pNode : vFloydPath )
        {
			result.push_back(Point(pNode->x, pNode->y));
        }
    }
}

void SilzAstar::makeGrid(const GameMap *map)
{
    int rows = map->getMapHeight();
    int cols = map->getMapWidth();

    _grid = new ASGrid(cols, rows);
	for ( int py = 0; py < rows; ++py )
	{
		for ( int px = 0; px < cols; ++px )
		{
            _grid->setWalkable(px, py, !(map->getCellData(px, py).obstacle));
		}
	}
	_grid->calculateLinks();
	_astar = new AStar(_grid);
}